#include "motor_sockets/can_socket.hpp"

MotorDrivers::CANSocket::CANSocket(const std::string& socketName, uint16_t recvID, uint16_t sendID, uint8_t canDLC) :
    socketName_(socketName),  recvID_(recvID), sendID_(sendID), canLength_(canDLC){
        frame.can_frame_.can_id = sendID_;
        frame.can_frame_.len = canLength_;
    };

MotorDrivers::CANSocket::~CANSocket(){
    if(readThread.joinable()) readThread.join();
}


bool MotorDrivers::CANSocket::socket_Init(){
    struct sockaddr_can addr;
    struct ifreq ifr;
    /// Configure Can Socket 配置Can口 ///
    socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    strcpy(ifr.ifr_name, socketName_.c_str());
    ioctl(socket_, SIOCGIFINDEX, &ifr);
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    /// Set Can Filter 设置Can滤波器 ///
    rfilter_[0].can_id   = recvID_;
    rfilter_[0].can_mask = CAN_SFF_MASK;
    setsockopt(socket_, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter_, sizeof(rfilter_));
    if (setsockopt(socket_, SOL_SOCKET, SO_RCVBUF, &MotorDrivers::BUFFER_SIZE, sizeof(MotorDrivers::BUFFER_SIZE)) == -1) {
            perror("setsockopt error");
            return false;
        }
    fcntl(socket_, F_SETFL, O_NONBLOCK);

    /// Export Socket 导出Can接口 ///
    if(bind(socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0){
        return false;
    }
    else
    {
        readThread = std::thread(&MotorDrivers::CANSocket::socket_ReadingThread, this);
        return true;
    }
}

int16_t MotorDrivers::CANSocket::socket_ReadMutation(uint8_t index){
    int16_t mutation_value;
    socket_PauseThread();
    mutation_value = mutation[index];
    mutation[index] = 0;
    socket_ResumeThread();
    return mutation_value;
    
}

void MotorDrivers::CANSocket::socket_DetectMutation(){
    if(data_buffer_.size() < 2) return;

    SocketFrame prv_socket_frame = data_buffer_.back();

    can_frame crt_frame = frame.can_frame_;
    can_frame prv_frame = prv_socket_frame.can_frame_;
    data_buffer_.pop_back();
    can_frame prv_prv_frame = data_buffer_.back().can_frame_;
    data_buffer_.push_back(prv_socket_frame);

    for(size_t i = 0; i < 8; i++){
        
        if(crt_frame.data[i] < prv_frame.data[i] && prv_frame.data[i] >= prv_prv_frame.data[i]
        && crt_frame.data[i] - prv_frame.data[i] < -5 && !mutation[i+8] )
        {
            mutation[i] ++;
            mutation[i+8] = 1;
            logCAN.logWrite("Mutation Detected: Data[" + 
            std::to_string(i) + "] ++" + std::to_string(mutation[i]));
        }
        else if(crt_frame.data[i] > prv_frame.data[i] && prv_frame.data[i] <= prv_prv_frame.data[i]
        && crt_frame.data[i] - prv_frame.data[i] > 5 && !mutation[i+8])
        {
            mutation[i]--;
            mutation[i+8] = 1;
            logCAN.logWrite("Mutation Detected: Data[" + 
            std::to_string(i) + "] --" + std::to_string(mutation[i]));
        }
        else mutation[i+8] = 0;
    }

}

void MotorDrivers::CANSocket::socket_ReadingThread(){
    fd_set readfds;
    struct timeval tv;
    int retval;
    while(true){

        while(!pause.load()) {

            FD_ZERO(&readfds);
            FD_SET(socket_, &readfds);

            tv.tv_usec = 10;

            retval = select(socket_ + 1, &readfds, nullptr, nullptr, &tv);

            if(retval == -1) perror("select error");
            else if(retval) {
                read(socket_, &frame.can_frame_, sizeof(can_frame));
                socket_DetectMutation();
                frame.timestamp_ = getTimeStamp();
                
                data_buffer_.push_back(frame);
                if(data_buffer_.size() > MAX_BUFFER_SIZE){
                    data_buffer_.pop_front();
                }
                CANLog();
            }
        }
    }
}

void MotorDrivers::CANSocket::socket_PauseThread(){
    pause = true;
}

void MotorDrivers::CANSocket::socket_ResumeThread(){
    pause = false;
}

MotorDrivers::SocketFrame MotorDrivers::CANSocket::socket_Read(){
    socket_PauseThread();
    MotorDrivers::SocketFrame frame = data_buffer_.back();
    data_buffer_.pop_back();
    socket_ResumeThread();
    return frame;
}

bool MotorDrivers::CANSocket::socket_Send(uint8_t* frame_data){
    socket_PauseThread();
    std::copy(frame_data, frame_data + frame.can_frame_.len, frame.can_frame_.data);
    std::cout << "Frame Data: ";
    for (int i = 0; i < frame.can_frame_.len; ++i) {
        std::cout << static_cast<int>(frame.can_frame_.data[i]) << " ";
    }
    std::cout << std::endl;
    if(write(socket_, &frame.can_frame_, sizeof(can_frame)) < 0){
        perror("socket send error");
        socket_ResumeThread();
        return false;
    }
    socket_ResumeThread();
    return true;
}

void MotorDrivers::CANSocket::CANLog(){
    logCAN.logWriteTimestamp();
    logCAN.logWrite("CAN log\n");
    logCAN.logWrite("CAN ID: " + std::to_string(frame.can_frame_.can_id) + "\n");
    logCAN.logWrite("CAN Data: ");
    for(int i = 0; i < frame.can_frame_.len; i++){
        logCAN.logWrite(std::to_string(frame.can_frame_.data[i]) + " ");
    }
    logCAN.logWrite("\n");
}